#include "bsp_tim.h"
#include "tim.h"
#include "dial.h"
#include "M3508.h"
#include "omni_wheel.h"
#include "dbus.h"
#include "imu.h"
#include "mahony.h"
#include "interface.h"
#include "can.h"
int led=1;

void bsp_tim_init()
{
	HAL_TIM_Base_Start_IT(&htim2);
	HAL_TIM_Base_Start_IT(&htim3);
	HAL_TIM_Base_Start_IT(&htim4);
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  if (htim->Instance == TIM2)
  {
//	    IMU_exe();   
		Mahony_exe();
  }
  
  if(htim->Instance == TIM3)
  {
	    HAL_GPIO_TogglePin(GPIOH,GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_12);
  }
  
  if(htim->Instance == TIM4)
  {
		if(Mahony_Data.Mahony_Filiter[2].flag==1)
		{
//			Quaternion_gimbal_cal(&q_to_gimbal,&Mahony_Data);
//			led=interface_Quaternion_cmd(&hcan1,&q_to_gimbal);
		}
  }
}
